#include "controller.h"

Controller::Controller()
{

}

void Controller::comm(Connection *comm)
{
    arduino = comm;


}



int Controller::getVals(char side, char dir)
{
    char *valString;
    std::string command, recieved;
    int val;

    command = "AAAG";
    command = command + side + dir;
    if(!arduino->send(command))
    {
        return -1;
    }

    recieved = arduino->read();

    std::cout << recieved << std::endl;

    valString = (char*) malloc(sizeof(char) * recieved.length() + 1);
    strcpy(valString, recieved.c_str());
    val = atoi(valString);

    return val;
}

int Controller::getDistance(char side, char fb)
{
    char *distString;
    std::string command, received;
    int dist;

    command = "AAAD";
    command = command + side + fb;

    if(!arduino->send(command))
    {
        return -1;
    }

    received = arduino->read();
    distString = (char*) malloc(sizeof(char) * received.length() + 1);
    strcpy(distString, received.c_str());
    dist = atoi(distString);

    return dist;
}

bool Controller::move(char direction)
{
    std::string command;

    command = "AAAM";
    command = command + direction;
    if(!arduino->send(command))
    {
        return false;
    }

    return true;
}

bool Controller::spin(char direction)
{
    std::string command;

    command = "AAAS";
    command = command + direction;
    if(!arduino->send(command))
    {
        return false;
    }

    return true;
}

void Controller::setVals(char side, char dir, int val)
{
    std::string command = "";
    command = "AAAV";
    command = command + side + dir;
    std::cout << command << std::endl;

    if(val < 1000)
    {
        char buffer[4];
        command += "0";
        itoa(val, buffer, 10);
        command += buffer;
        std::cout << command << std::endl;
    }
    else
    {
        char buffer[5];
        itoa(val, buffer, 10);
        command += buffer;
        std::cout << command << std::endl;
    }

    std::cout << command << std::endl;

    if(!arduino->send(command))
    {
        return;
    }
}

QString Controller::turret(int angle)
{
    std::string command;
    QString returnVal;

    command = "AAAA";

    if(angle < 1000)
    {
        char buffer[4];
        command += "0";
        itoa(angle, buffer, 10);
        command += buffer;
    }
    else
    {
        char buffer[5];
        itoa(angle, buffer, 10);
        command += buffer;
    }

    if(!arduino->send(command))
    {
        return false;
    }

    returnVal = returnVal.fromStdString(arduino->read());

    return returnVal;
}

int Controller::getUV()
{
    char *uvString;
    std::string command, received;
    int uv;

    command = "AAAU";
    //command = command + direction;
    if(!arduino->send(command))
    {
        return -1;
    }

    received = arduino->read();
    uvString = (char*) malloc(sizeof(char) * received.length() + 1);
    strcpy(uvString, received.c_str());
    uv = atoi(uvString);

    return uv;
}
